(add-load-path "../../mephisto/lib")
(add-load-path "../../mephisto/examples")

(use srfi-1)
(use gauche.sequence)
(use math.const)
(use gl.math3d)
(use mephisto.constraint)

(load "live-paint")
(load "mouse-handler")

(define (vector4f-length vec)
  (sqrt (vector4f-dot vec vec)))

(define (vector4f-scale vec factor)
  (list->vector4f (map (cut * <> factor) vec)))

(define r-ball (make-wire))
(define t-vec1 (make-wire))
(define t-vec2 (make-wire))
(define t-vec3 (make-wire))
(define t-vec4 (make-wire))
(define t-vec-thumb (make-wire))
(define center (make-wire))

;; (define len0 6)
;; (define p0->center (make-wire))
;; (define theta0 (make-wire))
;; (define quat0 (make-wire))
;; (define p0->p1 (make-wire))

;; (define phi (make-wire))
;; (attach-constraint! (theta0 => phi) (- pi theta0))

;; (define l0 (make-wire))
;; (attach-constraint!
;;  (p0->center theta0 => l0)
;;  (* (vector4f-length p0->center) (sin theta0)))

;; (define l1 (make-wire))
;; (attach-constraint!
;;  (l0 => l1)
;;  (- len0 l0))

(define (attach-finger-constraint len1 p1 p2 t-vec p1->p2 p1->center theta1 quat1)
  (attach-constraint!
   (quat1 p1 p1->center => p1->p2)
   (quatf-transform quat1 (vector4f-scale (vector4f-normalize p1->center) len1)))
  (attach-constraint! (p1 p1->p2 => p2) (point4f-add p1 p1->p2))
  (attach-constraint! (p1 center => p1->center) (point4f-sub center p1))
  (attach-constraint! (p1->center r-ball => theta1)
		      (acos (/ r-ball (vector4f-length p1->center))))
  (attach-constraint! (theta1 t-vec => quat1) (make-quatf t-vec (- theta1 pi/2)))
  )

;; (attach-constraint! (theta0 => quat0) (make-quatf t-vec (- theta0 pi/2)))
;; (attach-constraint!
;;  (p0 center => p0->center)
;;  (point4f-sub center p0))

;; (attach-constraint!
;;  (p0->center => theta0)
;;  (acos (/ r-ball (vector4f-length p0->center))))

;; (attach-constraint!
;;  (quat0 p0 p0->center => p0->p1)
;;  (quatf-transform quat0 (vector4f-scale (vector4f-normalize p0->center) len0)))
;; (attach-constraint!
;;  (p0 p0->p1 => p1)
;;  (point4f-add p0 p0->p1))

(define (make-finger-part length t-vec p-start p-end)
  (let-wires
   (ps->pe ps->center theta quat)
   (attach-finger-constraint length p-start p-end t-vec ps->pe ps->center theta quat)))

(define wrist (make-wire))

(define (make-finger wrist t-vec center)
  (let-wires
   (p1 p2 p3 p4)
   (make-finger-part 18 t-vec wrist p1)
   (make-finger-part 6 t-vec p1 p2)
   (make-finger-part 6 t-vec p2 p3)
   (make-finger-part 6 t-vec p3 p4)
   (list p1 p2 p3 p4)))

(define (make-thumb wrist t-vec center)
  (let-wires
   (p1 p2 p3)
   (make-finger-part 16 t-vec wrist p1)
   (make-finger-part 6 t-vec p1 p2)
   (make-finger-part 6 t-vec p2 p3)
   (list p1 p2 p3)))

(define finger1 (make-finger wrist t-vec1 center))
(define finger2 (make-finger wrist t-vec2 center))
(define finger3 (make-finger wrist t-vec3 center))
(define finger4 (make-finger wrist t-vec4 center))
(define thumb (make-thumb wrist t-vec-thumb center))

(define (main args)
  (mephisto-add-cut! (make-live-stream))

  (glut-init args)
  (glut-init-display-mode (logior GLUT_DOUBLE GLUT_DEPTH GLUT_RGB))

  ;; Game mode (full screen)
;;   (glut-game-mode-string "width=640 height=480 bpp~24 hertz=30")
;;   (glut-enter-game-mode)

  ;; Window
  (glut-init-window-size 640 480)
  (glut-init-window-position 0 0)
  (glut-create-window "MEPHISTO DEMO")

  (mephisto-init!)
  (gl-clear-color 0.0 0.3 1.0 0.0)

  (thread-start! (make-thread (lambda () (hand-main))))

  (glut-mouse-func mouse)
  (glut-main-loop)

  0)

(define (finger-stream finger t-vec)
  (let ((p1 (first finger))
	(p2 (second finger))
	(p3 (third finger))
	(p4 (fourth finger)))
    (procedure-stream
     (lambda/constraint
      (wrist p1 p2 p3 p4 t-vec)
      ((paint-matte-white
	(lambda ()
	  ((stick-painter wrist p1 t-vec 2))
	  ((stick-painter p1 p2 t-vec 2))
	  ((stick-painter p2 p3 t-vec 2))
	  ((stick-painter p3 p4 t-vec 2))))))
     )))

(define (thumb-stream finger t-vec)
  (let ((p1 (first finger))
	(p2 (second finger))
	(p3 (third finger)))
    (procedure-stream
     (lambda/constraint
      (wrist p1 p2 p3 t-vec)
      ((paint-matte-white
	(lambda ()
	  ((stick-painter wrist p1 t-vec 2))
	  ((stick-painter p1 p2 t-vec 2))
	  ((stick-painter p2 p3 t-vec 2))))))
     )))

(define (hand-main)
  (let* ((wrist-quat (make-quatf (vector4f 1 0 0) (/ pi 12)))
	 (wrist-quat-thumb (make-quatf (vector4f 1 0 0) (/ pi 4)))
	 (vec1 (vector4f 0 0 1))
	 (vec2 (quatf-transform wrist-quat vec1))
	 (vec3 (quatf-transform wrist-quat vec2))
	 (vec4 (quatf-transform wrist-quat vec3))
	 (vec-thumb (quatf-transform wrist-quat-thumb vec4)))
    (wire-set-value! t-vec1 vec1)
    (wire-set-value! t-vec2 vec2)
    (wire-set-value! t-vec3 vec3)
    (wire-set-value! t-vec4 vec4)
    (wire-set-value! t-vec-thumb vec-thumb))

  (wire-set-value! r-ball 27)
  (wire-set-value! wrist (point4f 0 0 0))
  (wire-set-value! center (point4f -30 0 0))

  (append-painter-streams!
   (stream-map
    (lambda (i)
      (lambda ()
	(when (and i (eq? (cadr i) GLUT_UP) (pair? *mouse-handle*))
	  ((car *mouse-handle*) i)
	  (set! *mouse-handle* (cdr *mouse-handle*)))))
    (make-input-stream))

   (stream (lambda ()
	     ;; camera control
	     (gl-matrix-mode GL_PROJECTION)
	     (gl-load-identity)
	     (gl-frustum -20 20 -15 15 50 150)
	     (gl-matrix-mode GL_MODELVIEW)
	     (gl-load-identity)
	     (glu-look-at 20.0 60.0 100.0 -20.0 10.0 0.0 0.0 1.0 0.0)
	     (gl-shade-model GL_SMOOTH)
	     ))

    (procedure-stream
     (lambda/constraint
      (r-ball center)
      ((move-to center
		(paint-yellow
		 (lambda () (glut-wire-sphere r-ball 16 16)))))))

   (finger-stream finger1 t-vec1)
   (finger-stream finger2 t-vec2)
   (finger-stream finger3 t-vec3)
   (finger-stream finger4 t-vec4)
   (thumb-stream thumb t-vec-thumb)
   )

  (add-mouse-handler!
   (lambda (args)
     (append-painter-streams!
      (procedure-stream
       (let1 p 0
	 (lambda ()
	   (inc! p)
	   (wire-set-value! r-ball (+ 26 (* 1 (sin (/ p 7)))))
;; 	   (wire-set-value! center (point4f (- -24 (* 2 (sin (/ p 7)))) 0 0))
;; 	   (wire-set-value! wrist (point4f (* 1 (sin (/ p 7))) 0 0))
	   ))
       ))))
)
